

var GRID, CTX;
var GRID_WIDTH = 600, GRID_HEIGHT = 600;
var obsX = 20, obsY = 360, obsVx = 10, obsVy = -6;
var obsRotationTheta = Math.atan2(obsVy, obsVx);
var obsRotationCosTheta = Math.cos(obsRotationTheta), obsRotationSinTheta = Math.sin(obsRotationTheta);
var goalX = 480, goalY = 480;
var robotX = 180, robotY = 180, robotVmax = 10;
var robotXDyn = robotX, robotYDyn = robotY, robotVxDyn = 0, robotVyDyn = 0;
var startTime = 0, endTime = 10000, stepTime = 100, time = startTime;
var uG = 1, uO = 1, uV = 0;
var running = false;
var timer;

function start(){
    var btnText = document.getElementById("startButton").value;
    if(timer != null){
        clearInterval(timer);
    }
    if(btnText == "Start"){
        resetValues();
        timer = setInterval("goStep();",100);
        document.getElementById("startButton").value = "Stop";
    }else{
        document.getElementById("startButton").value = "Start";
    }
    
    
}

function goStep(){
    time += stepTime;
    if(time >= endTime){
        clearInterval(timer);
        document.getElementById("startButton").value = "Start";
    }
    update();
}

function resetValues(){
    startTime = parseInt(document.getElementById("startTime").value);
    stepTime = parseInt(document.getElementById("stepTime").value);
    endTime = parseInt(document.getElementById("endTime").value);
    time = startTime;
    robotVxDyn = 0;
    robotVyDyn = 0;
    robotXDyn = robotX;
    robotYDyn = robotY;
    update();
}

function getGridX(x){
    return x + 20;
}
            
function getGridY(y){
    return GRID_HEIGHT - y - 20;
}
            
function init(){
    GRID = document.getElementById("grid");
    CTX = GRID.getContext("2d");
    CTX.save();
    document.getElementById("obstacleX").value = obsX;
    document.getElementById("obstacleY").value = obsY;
    document.getElementById("obstacleVx").value = obsVx;
    document.getElementById("obstacleVy").value = obsVy;
    document.getElementById("goalX").value = goalX;
    document.getElementById("goalY").value = goalY;
    document.getElementById("robotX").value = robotX;
    document.getElementById("robotY").value = robotY;
    document.getElementById("robotVmax").value = robotVmax;
    document.getElementById("startTime").value = startTime;
    document.getElementById("endTime").value = endTime;
    document.getElementById("stepTime").value = stepTime;
    document.getElementById("uG").value = uG;
    document.getElementById("uO").value = uO;
    document.getElementById("uV").value = uV;
    update();
}
            
function obstacleApplyData(){
    obsX = parseInt(document.getElementById("obstacleX").value);
    obsY = parseInt(document.getElementById("obstacleY").value);
    obsVx = parseInt(document.getElementById("obstacleVx").value);
    obsVy = parseInt(document.getElementById("obstacleVy").value);
    obsRotationTheta = Math.atan2(obsVy, obsVx);
    obsRotationSinTheta = Math.sin(obsRotationTheta);
    obsRotationCosTheta = Math.cos(obsRotationTheta);
    update();
}
            
function goalApplyData(){
    goalX =parseInt(document.getElementById("goalX").value);
    goalY =parseInt(document.getElementById("goalY").value);    
    update();
}
            
function robotApplyData(){
    robotX =parseInt(document.getElementById("robotX").value);
    robotY =parseInt(document.getElementById("robotY").value); 
    robotVmax =parseInt(document.getElementById("robotVmax").value); 
    update();
}

function applyForces(){
    uG = parseInt(document.getElementById("uG").value);
    uO = parseInt(document.getElementById("uO").value); 
    uV = parseInt(document.getElementById("uV").value); 
}
            
function update(){
    var obsXDyn = obsX + time * obsVx / 1000;
    var obsYDyn = obsY + time * obsVy / 1000;
    
    robotXDyn += robotVxDyn * stepTime / 1000;
    robotYDyn += robotVyDyn * stepTime / 1000;
    
    var robotXoDyn = obsRotationCosTheta * robotXDyn + obsRotationSinTheta * robotYDyn
    - obsRotationCosTheta * obsXDyn - obsRotationSinTheta * obsYDyn;
    var robotYoDyn = -obsRotationSinTheta * robotXDyn + obsRotationCosTheta * robotYDyn
    + obsRotationSinTheta * obsXDyn - obsRotationCosTheta * obsYDyn;
    var robotToDyn = Math.atan2(robotYoDyn, robotXoDyn);
    
    var obsXrDyn = obsRotationCosTheta * obsXDyn + obsRotationSinTheta * obsYDyn
    - obsRotationCosTheta * robotXDyn - obsRotationSinTheta * robotYDyn;
    var obsYrDyn = -obsRotationSinTheta * obsXDyn + obsRotationCosTheta * obsYDyn
    + obsRotationSinTheta * robotXDyn - obsRotationCosTheta * robotYDyn;
    var obsTrDyn = Math.atan2(obsYrDyn, obsXrDyn);
    
    
    var uX = (goalX > robotXDyn + 36 ? uG : (goalX < robotXDyn - 36 ? -uG : 0));
    var uY = (goalY > robotYDyn + 36 ? uG : (goalY < robotYDyn - 36 ? -uG : 0));
    
    var eX = Math.pow(Math.E, - Math.abs(obsXrDyn / 100));
    var eY = Math.pow(Math.E, - Math.abs(obsYrDyn / 100));
    var eV = Math.sqrt(eX * eX + eY * eY);
    
    //var relSinTheta = Math.sin(robotToDyn - obsRotationTheta);
    //var relCosTheta = Math.cos(robotToDyn - obsRotationTheta);
    
    var uVx = uO * (eX * obsRotationCosTheta + eY * obsRotationSinTheta);
    var uVy = uO * (eY * obsRotationCosTheta - eX * obsRotationSinTheta);
    
    //uX = (robotXoDyn > 0 ? uX - uVx : uX + uVx);
    //uY = (robotYoDyn > 0 ? uY + uVy : uY - uVy);
    
    uX += uVx;
    uY += uVy;
    
    var robotRotationDyn = Math.atan2(uY, uX);
    robotVxDyn = robotVmax * Math.cos(robotRotationDyn);
    robotVyDyn = robotVmax * Math.sin(robotRotationDyn);
    
        
    CTX.restore();   
    CTX.clearRect(0, 0, GRID_WIDTH, GRID_HEIGHT);
    drawAxis(0, 0, 240, 0, "Wx", "Wy");
    drawObject(goalX, goalY, Math.PI / 2, document.getElementById("goalIcon"));
    drawObject(obsXDyn, obsYDyn, Math.atan2(obsVy, obsVx), document.getElementById("obstacleIcon"));
    drawObject(robotXDyn, robotYDyn, robotRotationDyn, document.getElementById("robotIcon"));  
    drawAxis(robotXDyn, robotYDyn, 100, 0, robotVxDyn.toFixed(0), robotVyDyn.toFixed(0));
    
    document.getElementById("obsXDynCell").innerHTML = obsXDyn.toFixed(0);
    document.getElementById("obsYDynCell").innerHTML = obsYDyn.toFixed(0);
    document.getElementById("robotXDynCell").innerHTML = robotXDyn.toFixed(0);
    document.getElementById("robotYDynCell").innerHTML = robotYDyn.toFixed(0);
    document.getElementById("robotVxDynCell").innerHTML = robotVxDyn.toFixed(0);
    document.getElementById("robotVyDynCell").innerHTML = robotVyDyn.toFixed(0);
    document.getElementById("robotXoDynCell").innerHTML = robotXoDyn.toFixed(0);
    document.getElementById("robotYoDynCell").innerHTML = robotYoDyn.toFixed(0);    
}
            
function drawObject(x, y, theta, image){
    theta = Math.PI / 2 -theta;
    var cosTheta = Math.cos(theta);
    var sinTheta = Math.sin(theta);
    x = getGridX(x);
    y = getGridY(y);
    var nx = y * sinTheta + x * cosTheta - 24;
    var ny = y * cosTheta - x * sinTheta - 24;
    CTX.save();
    CTX.rotate(theta);
    CTX.drawImage(image, nx, ny);
    CTX.restore();   
}
            
function drawAxis(x, y, size, theta, xLabel, yLabel){
    x = getGridX(x);
    y = getGridY(y);
    var sizeCosTheta = size * Math.cos(theta);
    var sizeSinTheta = size * Math.sin(theta);
    CTX.save();
    CTX.font="12px Arial";
    CTX.beginPath();
    CTX.moveTo(x,y);
    CTX.lineTo(x + sizeCosTheta, y - sizeSinTheta);
    CTX.stroke();
    CTX.fillText(xLabel, x + sizeCosTheta, y - sizeSinTheta);
    CTX.moveTo(x,y);
    CTX.lineTo(x - sizeSinTheta,y - sizeCosTheta);
    CTX.stroke();
    CTX.fillText(yLabel, x - sizeSinTheta,y - sizeCosTheta);
    CTX.restore();            
}

